# Copyright (c) 2025 lotusymt
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# AI-assisted: initial draft generated by an AI tool; fully reviewed and edited by the author.

import time

from geometry_msgs.msg import Point, Pose, Quaternion, Twist
from nav2_msgs.msg import Costmap, CostmapMetaData
import numpy as np
import rclpy
from rclpy.node import Node
from rosgraph_msgs.msg import Clock as ClockMsg
from std_msgs.msg import Header

# --- standardized imports ---
# This node is meant to *mimic* what a recorded bag would provide:
# - /clock: so the rest of the system can run in sim time
# - /cmd_vel_smoothed: so there is a "robot is trying to move" signal
# - /local_costmap/costmap: so Collision Monitor sees an obstacle only in a time window
#
# Test intent:
#   0..3s   → no obstacle → robot should keep going
#   3..8s   → obstacle right under robot → CM should stop it
#   8s..+   → obstacle gone → CM should let it move again
# This must match the YAML used by the real CM test.


class FakeCMSource(Node):

    def __init__(self) -> None:
        super().__init__('fake_cm_bag_source')

        # Publishers
        self.clock_pub = self.create_publisher(ClockMsg, '/clock', 10)
        self.costmap_pub = self.create_publisher(
            Costmap, '/local_costmap/costmap', 10)
        self.cmd_pub = self.create_publisher(Twist, '/cmd_vel_smoothed', 10)

        # Simple 5m x 5m costmap centered on robot, 0.05 m resolution (100x100)
        self.res = 0.05
        self.size_x = 100
        self.size_y = 100
        self.origin = Pose(
            position=Point(x=-2.5, y=-2.5, z=0.0),
            orientation=Quaternion(w=1.0)
        )

        self.t0_ns = time.monotonic_ns()

        self.timer = self.create_timer(0.05, self.tick)  # 20 Hz

    def tick(self) -> None:
        # 1) /clock
        # in tick()
        now_ns = time.monotonic_ns() - self.t0_ns
        now_s = now_ns / 1e9  # convert to seconds
        clk = ClockMsg()
        clk.clock.sec = now_ns // 1_000_000_000
        clk.clock.nanosec = now_ns % 1_000_000_000
        self.clock_pub.publish(clk)

        # 2) /cmd_vel_smoothed: small forward velocity
        t = Twist()
        t.linear.x = 0.20
        self.cmd_pub.publish(t)

        # 3) /local_costmap/costmap
        #    0..3s SAFE → 3..8s DANGER (lethal cell at robot) → 8s+ SAFE (recover)
        cm = Costmap()
        cm.header = Header()
        cm.header.stamp = clk.clock
        cm.header.frame_id = 'base_footprint'  # match YAML base_frame_id

        meta = CostmapMetaData()
        meta.map_load_time = clk.clock
        meta.resolution = float(self.res)
        meta.size_x = self.size_x
        meta.size_y = self.size_y
        meta.origin = self.origin
        cm.metadata = meta

        # Start with all-free costmap
        data = np.zeros((self.size_y, self.size_x), dtype=np.uint8)

        if 3.0 <= now_s <= 8.0:
            # During the "danger window", make the cell under the robot lethal.
            # This is what should make CM stop the robot.
            cx = self.size_x // 2
            cy = self.size_y // 2
            data[cy, cx] = 254  # lethal

        cm.data = data.flatten().tolist()
        self.costmap_pub.publish(cm)


def main(args: list[str] | None = None) -> None:
    rclpy.init()
    node = FakeCMSource()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()
